/*
Copyright (C) 2021-2025 Casa Xu (also Zhiyan Xu) from HIT

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

/*
 * LOS_DEBUG.c
 *
 *  Created on: 2023年3月6日
 *      Author: 64435
 */

#include "LOS_DEBUG.h"

uint8_t DYN_PACK[420];
uint8_t TAB_PACK[700];


void LOS_DEBUG_SendMsg(uint8_t* msg, uint16_t len)
{
    LOS_CAN_SendPack(LOS_CAN_NUM2, 0xFC, msg, len);
}

// void LOS_DEBUG_DynTest()
// {
//     uint16_t i = 0;
//     LOS_DEBUG_DynShortPack(DYN_PACK);
//     LOS_MAX14830_WriteBytes(DEBUG_MAXNUM,DEBUG_MAXPort,DYN_PACK,100);
//     LOS_MAX14830_WriteBytes(DEBUG_MAXNUM,DEBUG_MAXPort,&DYN_PACK[100],100);
//     LOS_MAX14830_WriteBytes(DEBUG_MAXNUM,DEBUG_MAXPort,&DYN_PACK[200],100);
//     LOS_MAX14830_WriteBytes(DEBUG_MAXNUM,DEBUG_MAXPort,&DYN_PACK[300],100);
//     //sciSend(sciREG4,400,DYN_PACK);
// }



// /** @fn void LOS_DEBUG_DynMsgSend(uint8_t* tmmsg)
// *   @brief 动力学联试发送函数
// *   @param[in] dynmsg 
// *   @note 
// */
// void LOS_DEBUG_DynMsgSend(uint8_t* dynmsg)  
// {
//     LOS_SNP_t SNP_CMD;    //首部配置结构体
//     uint8_t FP_CMD[8];    //首部内容，首部用完以后给其他帧做缓冲区使用
//     uint16_t cnt = 0;
//     uint8_t frame_cnt = 3; //帧计数
//     uint16_t delay = 0;

//     SNP_CMD.SNP_dest = 0xDD;            //动力学联试专用
//     SNP_CMD.SNP_source = 0xF0;          //OBC发送
//     SNP_CMD.SNP_rec = 0x00;             //内容识别码：
//     SNP_CMD.SNP_len = 436;              //长度（包含首部）
//     SNP_CMD.SNP_tran = 0x06;            //0110B,信息传递服务
//     LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);
//     LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, SNP_CMD.SNP_dest, 0X00), FP_CMD, 8);//发送首部
//     for(cnt=0;cnt<8;cnt++)
//     {
//         FP_CMD[cnt] = 0xFF;
//     }
//     for (delay = 0;delay<1000;delay++);
//     LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, SNP_CMD.SNP_dest, 0X01), FP_CMD, 8); //前10字节要被丢弃，填上FFH，这是8个
//     for(cnt=2;cnt<8;cnt++)
//     {
//         FP_CMD[cnt] = dynmsg[cnt-2];
//     }
//     LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, SNP_CMD.SNP_dest, 0X02), FP_CMD, 8); //前10字节要被丢弃，填上FFH，这是2个,8+2=10，完美
//     SNP_CMD.SNP_len -= 24; //发了3帧，长度减24
//     while(SNP_CMD.SNP_len > 4)
//     {
//         for (delay = 0;delay<1000;delay++);
//         LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, SNP_CMD.SNP_dest, frame_cnt), &dynmsg[cnt-2], 8);
//         cnt += 8;
//         SNP_CMD.SNP_len -= 8;
//         frame_cnt += 1;
//     }
//     FP_CMD[0]=dynmsg[cnt-2];
//     FP_CMD[1]=dynmsg[cnt-1];
//     FP_CMD[2]=0xFF;  //最后2字节舍弃
//     FP_CMD[3]=0xFF;
//     LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, SNP_CMD.SNP_dest, 0XFF), FP_CMD, 4);
// }


// /** @fn void LOS_DEBUG_DynPack(uint8_t* tmmsg)
// *   @brief 动力学联试组包函数
// *   @param[in] tmmsg 遥测包内容
// *   @note 根据遥测协议进行组包
// */
// void LOS_DEBUG_DynPack(uint8_t* tmmsg)
// {
//     //2
//     tmmsg[0]=0xDD;
//     tmmsg[1]=STA_FW[1].TM_MOD;
//     memcpy(&tmmsg[2],&(STA_FW[0].TM_RPM),4);
//     //3
//     memcpy(&tmmsg[6],&(STA_FW[1].TM_RPM),4);
//     memcpy(&tmmsg[10],&(STA_FW[2].TM_RPM),4);
//     //4
//     memcpy(&tmmsg[14],&LOS_RealTime_Year,2);
//     memcpy(&tmmsg[16],&LOS_RealTime_Month,1);
//     memcpy(&tmmsg[17],&LOS_RealTime_Day,1);
//     memcpy(&tmmsg[18],&LOS_RealTime_Hour,1);
//     memcpy(&tmmsg[19],&LOS_RealTime_Minute,1);
//     memcpy(&tmmsg[20],&LOS_RealTime_Second,1);
//     memcpy(&tmmsg[21],&STA_YH50.VAL,1);
//     //5
//     memcpy(&tmmsg[22],&STA_YH50.x_gyro,8);
//     //6
//     memcpy(&tmmsg[30],&STA_YH50.y_gyro,8);
//     //7
//     memcpy(&tmmsg[38],&STA_YH50.z_gyro,8);
//     //8
//     memcpy(&tmmsg[46],&aocs_g_output_struct.RW_Tcmd[0],8); //飞轮力矩
//     //9
//     memcpy(&tmmsg[54],&aocs_g_output_struct.RW_Tcmd[1],8);
//     //10
//     memcpy(&tmmsg[62],&aocs_g_output_struct.RW_Tcmd[2],8);
//     //11
//     memcpy(&tmmsg[70],&aocs_g_output_struct.RW_Tcmd[3],8);
//     //12
//     memcpy(&tmmsg[78],&STA_SSZ.q[0],8);
//     //13
//     memcpy(&tmmsg[86],&STA_SSZ.q[1],8);
//     //14 
//     memcpy(&tmmsg[94],&STA_SSZ.q[2],8);
//     //15
//     memcpy(&tmmsg[102],&STA_SSZ.q[3],8);
//     //16
//     memcpy(&tmmsg[110],&STA_GNSS.sec, 4);
//     memcpy(&tmmsg[114],&(STA_FW[3].TM_RPM),4);
//     //17
//     memcpy(&tmmsg[118], aocs_g_output_struct.MGT_Mcmd, 6);
//     //18
    
//     //19
//     memcpy(&tmmsg[134],&STA_SUNS.alpha,8);
//     //20
//     memcpy(&tmmsg[142],&STA_SUNS.beta,8);
//     //21
//     memcpy(&tmmsg[150],&STA_MAGME.Field[0],8);
//     //22
//     memcpy(&tmmsg[158],&STA_MAGME.Field[1],8);
//     //23
//     memcpy(&tmmsg[166],&STA_MAGME.Field[2],8);
//     //24
//     memcpy(&tmmsg[174],&STA_SSY.q[0],8);
//     //25
//     memcpy(&tmmsg[182],&STA_SSY.q[1],8);
//     //26
//     memcpy(&tmmsg[190],&STA_SSY.q[2],8);
//     //27
//     memcpy(&tmmsg[198],&STA_SSY.q[3],8);
//     //28 //29 //30
//     memcpy(&tmmsg[206], &STA_GNSS.pos_vel[0], 24);
//     //31 //32 //33
//     memcpy(&tmmsg[230], &STA_GNSS.orbit[0], 24);
//     //34 //35 //36
//     memcpy(&tmmsg[254], aocs_g_output_struct.B_i, 24);
//     //37 //38 //39
//     memcpy(&tmmsg[278], aocs_g_output_struct.S_i, 24);
//     //40
//     memcpy(&tmmsg[302], &aocs_g_input_struct.GNSS_time, 8);
//     //41
//     memcpy(&tmmsg[310], &LOS_Time_Double, 8);
//     //42
//     memcpy(&tmmsg[318], &aocsentertime, 4);
//     memcpy(&tmmsg[322], &aocsexittime, 4);
//     //43
//     memcpy(&tmmsg[326], &aocstotaltime, 4);
//     memcpy(&tmmsg[330], &LOS_ModeAcsIn,2);
//     //44
//     memcpy(&tmmsg[334], &LOS_Targetq[0], 8);
//     //45
//     memcpy(&tmmsg[342], &LOS_Targetq[2], 8);
//     //46
//     memcpy(&tmmsg[350], &LOS_Targetpos[0], 8);
//     //47
//     memcpy(&tmmsg[358], &LOS_Targetpos[2], 4);
//     memcpy(&tmmsg[362], &STA_MEMS.gyro_x, 4);
//     //48
//     memcpy(&tmmsg[366], &STA_MEMS.gyro_y, 4);
//     memcpy(&tmmsg[370], &STA_MEMS.gyro_z, 4);
//     //49 //50
//     memcpy(&tmmsg[374],&aocs_g_output_struct.B_i[6],16);
//     //51 //52
//     memcpy(&tmmsg[390],&aocs_g_output_struct.S_i[6],16);
       
// }

// /** @fn void LOS_DEBUG_DynShortPack(uint8_t* tmmsg)
// *   @brief 动力学联试组包函数
// *   @param[in] tmmsg 遥测包内容
// *   @note 试图组出一个更短的包
// */
// void LOS_DEBUG_DynShortPack(uint8_t* tmmsg)
// {
//     float fbuf = 0;

//     tmmsg[0]=0xDD;
//     tmmsg[1]=0xDD;

//     memcpy(&tmmsg[2],&(STA_FW[0].TM_RPM),4);
//     memcpy(&tmmsg[6],&(STA_FW[1].TM_RPM),4);
//     memcpy(&tmmsg[10],&(STA_FW[2].TM_RPM),4);

//     //实时时间
//     memcpy(&tmmsg[14],&LOS_RealTime_Year,2);
//     memcpy(&tmmsg[16],&LOS_RealTime_Month,1);
//     memcpy(&tmmsg[17],&LOS_RealTime_Day,1);
//     memcpy(&tmmsg[18],&LOS_RealTime_Hour,1);
//     memcpy(&tmmsg[19],&LOS_RealTime_Minute,1);
//     memcpy(&tmmsg[20],&LOS_RealTime_Second,1);

//     tmmsg[21]=0xDD;
    
//     //光纤陀螺
//     fbuf = STA_YH50.x_gyro;
//     memcpy(&tmmsg[22],&fbuf,4);
//     fbuf = STA_YH50.y_gyro;
//     memcpy(&tmmsg[26],&fbuf,4);
//     fbuf = STA_YH50.z_gyro;
//     memcpy(&tmmsg[30],&fbuf,4);

//     //飞轮力矩
//     fbuf = aocs_g_output_struct.RW_Tcmd[0];
//     memcpy(&tmmsg[34],&fbuf,4); 
//     fbuf = aocs_g_output_struct.RW_Tcmd[1];
//     memcpy(&tmmsg[38],&fbuf,4);
//     fbuf = aocs_g_output_struct.RW_Tcmd[2];
//     memcpy(&tmmsg[42],&fbuf,4);
//     fbuf = aocs_g_output_struct.RW_Tcmd[3];
//     memcpy(&tmmsg[46],&fbuf,4);
    
//     //星敏Z
//     fbuf = STA_SSZ.q[0];
//     memcpy(&tmmsg[50],&fbuf,4);
//     fbuf = STA_SSZ.q[1];
//     memcpy(&tmmsg[54],&fbuf,4);
//     fbuf = STA_SSZ.q[2];
//     memcpy(&tmmsg[58],&fbuf,4);
//     fbuf = STA_SSZ.q[3];
//     memcpy(&tmmsg[62],&fbuf,4);
    
//     //飞轮S转速
//     memcpy(&tmmsg[66],&(STA_FW[3].TM_RPM),4);

//     //磁力矩器占空比
//     memcpy(&tmmsg[70], aocs_g_output_struct.MGT_Mcmd, 6);
    
//     //太敏
//     fbuf = STA_SUNS.alpha;
//     memcpy(&tmmsg[76],&fbuf,4);
//     fbuf = STA_SUNS.beta;
//     memcpy(&tmmsg[80],&fbuf,4);

//     //磁强计
//     fbuf = STA_MAGME.Field[0];
//     memcpy(&tmmsg[84],&fbuf,4);
//     fbuf = STA_MAGME.Field[1];
//     memcpy(&tmmsg[88],&fbuf,4);
//     fbuf = STA_MAGME.Field[2];
//     memcpy(&tmmsg[92],&fbuf,4);

//     //星敏Y
//     fbuf = STA_SSY.q[0];
//     memcpy(&tmmsg[96],&fbuf,4);
//     fbuf = STA_SSY.q[1];
//     memcpy(&tmmsg[100],&fbuf,4);
//     fbuf = STA_SSY.q[2];
//     memcpy(&tmmsg[104],&fbuf,4);
//     fbuf = STA_SSY.q[3];
//     memcpy(&tmmsg[108],&fbuf,4);

//     //GNSS
//     memcpy(&tmmsg[112], &STA_GNSS.pos_vel[0], 24);
//     memcpy(&tmmsg[136], &STA_GNSS.orbit[0], 24);
    
//     //过程量
//     memcpy(&tmmsg[160], aocs_g_output_struct.B_i, 24);
//     memcpy(&tmmsg[184], aocs_g_output_struct.S_i, 24);

//     //进入姿控程序次数
//     memcpy(&tmmsg[208], &AOCS_Enter_Times, 4);

//     //工作模式
//     memcpy(&tmmsg[212], &LOS_ModeAcsIn,2);

//     //目标四元数
//     memcpy(&tmmsg[214], &LOS_Targetq[0], 8);
//     memcpy(&tmmsg[222], &LOS_Targetq[2], 8);

//     //目标地理位置
//     memcpy(&tmmsg[230], &LOS_Targetpos[0], 8);
//     memcpy(&tmmsg[238], &LOS_Targetpos[2], 4);

//     //MEMS陀螺
//     memcpy(&tmmsg[242], &STA_MEMS.gyro_x, 4);
//     memcpy(&tmmsg[246], &STA_MEMS.gyro_y, 4);
//     memcpy(&tmmsg[250], &STA_MEMS.gyro_z, 4);

//     //过程量
//     memcpy(&tmmsg[254],&aocs_g_output_struct.B_i[6],16);    
//     memcpy(&tmmsg[270],&aocs_g_output_struct.S_i[6],16);

//     //OBC累计运行时间
//     memcpy(&tmmsg[286],&LOS_Time_SecFromBoot,4);

//     //飞轮最后一次设置量
//     memcpy(&tmmsg[290], &STA_FW[0].TM_LSET, 4);
//     memcpy(&tmmsg[294], &STA_FW[1].TM_LSET, 4);
//     memcpy(&tmmsg[298], &STA_FW[2].TM_LSET, 4);
//     memcpy(&tmmsg[302], &STA_FW[3].TM_LSET, 4);
//     //卫星接收指令计数
//     memcpy(&tmmsg[306], &LOS_TC_READ, 2);
//     //卫星执行指令计数
//     memcpy(&tmmsg[308], &LOS_TC_EX, 2);
//     //上一条执行指令ID    
//     memcpy(&tmmsg[310], &LOS_TC_LASTCMD, 4);
//     //星敏Y开关
//     tmmsg[314] = STA_EPS.SWITCH_TC[12];
//     //星敏Z开关
//     tmmsg[315] = STA_EPS.SWITCH_TC[13];
//     //太敏开关
//     tmmsg[316] = STA_EPS.SWITCH_PD[11];
//     //光纤陀螺开关
//     tmmsg[317] = STA_EPS.SWITCH_TC[15];
//     //MEMS陀螺开关
//     tmmsg[318] = STA_EPS.SWITCH_PD[19];
//     //GNSS开关
//     tmmsg[319] = STA_EPS.SWITCH_PD[7];
//     //飞轮X开关
//     tmmsg[320] = STA_EPS.SWITCH_PD[12];
//     //飞轮Y开关
//     tmmsg[321] = STA_EPS.SWITCH_PD[13];
//     //飞轮Z开关
//     tmmsg[322] = STA_EPS.SWITCH_PD[14];
//     //飞轮S开关
//     tmmsg[323] = STA_EPS.SWITCH_PD[15];
//     //测试模式标志
//     tmmsg[324] = LOS_TESTM_FLAG;
//     //星敏Y物理量异常复位次数
//     memcpy(&tmmsg[325], &STA_SSY.FlagF_CNT, 2);
//     //星敏Y通信异常复位次数
//     memcpy(&tmmsg[327], &STA_SSY.FlagT_CNT, 2);
//     //星敏Z物理量异常复位次数
//     memcpy(&tmmsg[329], &STA_SSZ.FlagF_CNT, 2);
//     //星敏Z通信异常复位次数
//     memcpy(&tmmsg[331], &STA_SSZ.FlagT_CNT, 2);
//     //光纤陀螺物理量异常复位次数
//     memcpy(&tmmsg[333], &STA_YH50.FlagF_CNT, 2);
//     //光纤陀螺通信异常复位次数
//     memcpy(&tmmsg[335], &STA_YH50.FlagT_CNT, 2);
//     //MEMS物理量异常复位次数
//     memcpy(&tmmsg[337], &STA_MEMS.FlagF_CNT, 2);
//     //MEMS通信异常复位次数
//     memcpy(&tmmsg[339], &STA_MEMS.FlagT_CNT, 2);
//     //GNSS物理量异常复位次数
//     memcpy(&tmmsg[341], &STA_GNSS.FlagF_CNT, 2);
//     //GNSS通信异常复位次数
//     memcpy(&tmmsg[343], &STA_GNSS.FlagT_CNT, 2);
//     //飞轮X物理量异常复位次数
//     memcpy(&tmmsg[345], &STA_FW[FW_X].FlagF_CNT, 2);
//     //飞轮X通信异常复位次数
//     memcpy(&tmmsg[347], &STA_FW[FW_X].FlagT_CNT, 2);
//     //飞轮Y物理量异常复位次数
//     memcpy(&tmmsg[349], &STA_FW[FW_Y].FlagF_CNT, 2);
//     //飞轮Y通信异常复位次数
//     memcpy(&tmmsg[351], &STA_FW[FW_Y].FlagT_CNT, 2);
//     //飞轮Z物理量异常复位次数
//     memcpy(&tmmsg[353], &STA_FW[FW_Z].FlagF_CNT, 2);
//     //飞轮Z通信异常复位次数
//     memcpy(&tmmsg[355], &STA_FW[FW_Z].FlagT_CNT, 2);
//     //飞轮S物理量异常复位次数
//     memcpy(&tmmsg[357], &STA_FW[FW_S].FlagF_CNT, 2);
//     //飞轮S通信异常复位次数
//     memcpy(&tmmsg[359], &STA_FW[FW_S].FlagT_CNT, 2);
//     //磁强计开关
//     tmmsg[361] = STA_EPS.SWITCH_TC[14];
//     //磁强计物理量异常复位次数
//     memcpy(&tmmsg[362], &STA_MAGME.FlagF_CNT, 2);
//     //磁强计通信异常复位次数
//     memcpy(&tmmsg[364], &STA_MAGME.FlagT_CNT, 2);
//     //太敏物理量异常复位次数
//     memcpy(&tmmsg[366], &STA_SUNS.FlagF_CNT, 2);
//     //太敏通信异常复位次数
//     memcpy(&tmmsg[368], &STA_SUNS.FlagT_CNT, 2);

//     //星敏Y Y异常复位次数
//     memcpy(&tmmsg[370], &STA_SSY.FlagY_CNT, 2);
//     //星敏Z Y异常复位次数
//     memcpy(&tmmsg[372], &STA_SSZ.FlagY_CNT, 2);
//     //光纤陀螺 Y异常复位次数
//     memcpy(&tmmsg[374], &STA_YH50.FlagY_CNT, 2);
//     //MEMS Y异常复位次数
//     memcpy(&tmmsg[376], &STA_MEMS.FlagY_CNT, 2);
//     //GNSS Y异常复位次数
//     memcpy(&tmmsg[378], &STA_GNSS.FlagY_CNT, 2);
//     //飞轮X Y异常复位次数
//     memcpy(&tmmsg[380], &STA_FW[FW_X].FlagY_CNT, 2);
//     //飞轮Y Y异常复位次数
//     memcpy(&tmmsg[382], &STA_FW[FW_Y].FlagY_CNT, 2);
//     //飞轮Z Y异常复位次数
//     memcpy(&tmmsg[384], &STA_FW[FW_Z].FlagY_CNT, 2);
//     //飞轮S Y异常复位次数
//     memcpy(&tmmsg[386], &STA_FW[FW_S].FlagY_CNT, 2);
//     //磁强计 Y异常复位次数
//     memcpy(&tmmsg[388], &STA_MAGME.FlagY_CNT, 2);
//     //太敏 Y异常复位次数
//     memcpy(&tmmsg[390], &STA_SUNS.FlagY_CNT, 2);

// }



void LOS_DEBUG_TabTest()
{
    LOS_DEBUG_TestPack0(TAB_PACK);
    //sciSend(sciREG4,450,TAB_PACK);
    LOS_MAX14830_WriteBytes(DEBUG_MAXNUM,DEBUG_MAXPort,TAB_PACK,100);
    LOS_MAX14830_WriteBytes(DEBUG_MAXNUM,DEBUG_MAXPort,&TAB_PACK[100],100);
    LOS_MAX14830_WriteBytes(DEBUG_MAXNUM,DEBUG_MAXPort,&TAB_PACK[200],100);
    LOS_MAX14830_WriteBytes(DEBUG_MAXNUM,DEBUG_MAXPort,&TAB_PACK[300],100);
    LOS_MAX14830_WriteBytes(DEBUG_MAXNUM,DEBUG_MAXPort,&TAB_PACK[400],100);
    LOS_MAX14830_WriteBytes(DEBUG_MAXNUM,DEBUG_MAXPort,&TAB_PACK[500],100);
    LOS_MAX14830_WriteBytes(DEBUG_MAXNUM,DEBUG_MAXPort,&TAB_PACK[600],100);
}

void LOS_DEBUG_CAM()
{
    sciSend(sciREG4,39,TAB_PACK);
}

/** @fn void LOS_DEBUG_TabMsgSend(uint8_t* tmmsg)
*   @brief 桌面联试发送函数
*   @param[in] dynmsg 
*   @note 
*/
void LOS_DEBUG_TabMsgSend(uint8_t* dynmsg)  
{
    LOS_SNP_t SNP_CMD;    //首部配置结构体
    uint8_t FP_CMD[8];    //首部内容，首部用完以后给其他帧做缓冲区使用
    uint16_t cnt = 0;
    uint8_t frame_cnt = 3; //帧计数
    uint16_t delay = 0;

    SNP_CMD.SNP_dest = 0xEE;            //桌面联试专用
    SNP_CMD.SNP_source = 0xF0;          //OBC发送
    SNP_CMD.SNP_rec = 0x00;             //内容识别码：
    SNP_CMD.SNP_len = 356;              //长度（包含首部）
    SNP_CMD.SNP_tran = 0x06;            //0110B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, SNP_CMD.SNP_dest, 0X00), FP_CMD, 8);//发送首部
    for(cnt=0;cnt<8;cnt++)
    {
        FP_CMD[cnt] = 0xFF;
    }
    for (delay = 0;delay<1000;delay++);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, SNP_CMD.SNP_dest, 0X01), FP_CMD, 8); //前10字节要被丢弃，填上FFH，这是8个
    for(cnt=2;cnt<8;cnt++)
    {
        FP_CMD[cnt] = dynmsg[cnt-2];
    }
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, SNP_CMD.SNP_dest, 0X02), FP_CMD, 8); //前10字节要被丢弃，填上FFH，这是2个,8+2=10，完美
    SNP_CMD.SNP_len -= 24; //发了3帧，长度减24
    while(SNP_CMD.SNP_len > 4)
    {
        LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, SNP_CMD.SNP_dest, frame_cnt), &dynmsg[cnt-2], 8);
        cnt += 8;
        SNP_CMD.SNP_len -= 8;
        frame_cnt += 1;
    }
    FP_CMD[0]=dynmsg[cnt-2];
    FP_CMD[1]=dynmsg[cnt-1];
    FP_CMD[2]=0xFF;  //最后2字节舍弃
    FP_CMD[3]=0xFF;
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, SNP_CMD.SNP_dest, 0XFF), FP_CMD, 4);
}


// /** @fn void LOS_DEBUG_TabPack(uint8_t* tmmsg)
// *   @brief 电源联试组包函数
// *   @param[in] tmmsg 遥测包内容
// *   @note 根据遥测协议进行组包
// */
// void LOS_DEBUG_TabPack(uint8_t* tmmsg)
// {
//     tmmsg[0] = 0xEE;
//     memcpy(&tmmsg[1],STA_EPS.Vol_MPPT,6);//

//     memcpy(&tmmsg[7],STA_EPS.SWITCH_PD,24);//

//     memcpy(&tmmsg[31],STA_EPS.SWITCH_TC,24);//

//     memcpy(&tmmsg[55],STA_EPS.Cur_PD25940,48);//

//     memcpy(&tmmsg[103],STA_EPS.Cur_TC25940,48);//

//     tmmsg[151] = STA_EPS.Cur_PDMSP430;
//     tmmsg[152] = STA_EPS.Cur_TCMSP430;
//     tmmsg[153] = STA_EPS.ball;
//     memcpy(&tmmsg[154], &STA_EPS.Temp_3V3, 1);
//     memcpy(&tmmsg[155], &STA_EPS.Temp_5V, 1);
//     memcpy(&tmmsg[156], &STA_EPS.Temp_12V, 1);
//     memcpy(&tmmsg[157], &STA_EPS.Vol_3V3, 2);

//     memcpy(&tmmsg[159], &STA_EPS.Temp_MPPT[0], 3);
//     memcpy(&tmmsg[162], &STA_EPS.Temp_Charger[0], 2); 
//     memcpy(&tmmsg[164], &STA_EPS.Vol_5V, 2);
   
//     memcpy(&tmmsg[166], STA_EPS.Vol_MPPT, 6);
//     memcpy(&tmmsg[172], STA_EPS.Vol_Charger, 2);

//     memcpy(&tmmsg[174], &STA_EPS.Vol_Charger[1], 2);
//     memcpy(&tmmsg[176], STA_EPS.Cur_MPPT, 6);

//     memcpy(&tmmsg[182], &STA_EPS.Vol_12V, 2);
//     memcpy(&tmmsg[184], STA_EPS.Cur_Charger, 4);
//     memcpy(&tmmsg[188], &STA_EPS.Cur_3V3, 2);

//     memcpy(&tmmsg[190], &STA_EPS.Cur_5V, 2);
//     memcpy(&tmmsg[192], &STA_EPS.Cur_12V, 2);
//     memcpy(&tmmsg[194], STA_EPS.Vol_SA, 4);

//     memcpy(&tmmsg[198], &STA_EPS.Vol_SA[2], 2);

//     memcpy(&tmmsg[200], &LOS_Time_SecFromBoot, 4);

//     memcpy(&tmmsg[204], &STA_EPS.Temp_Ther, 32);
//     //memcpy(&tmmsg[200], STA_EPS.Temp_Ther, 32);

//     //memcpy(&tmmsg[232], STA_EPS.Vcell, 12);
// }


/** @fn void LOS_DEBUG_TestPack0(uint8_t* tmmsg)
*   @brief 桌面联试组包函数
*   @param[in] tmmsg 遥测包内容
*   @note 根据遥测协议进行组包
*/
void LOS_DEBUG_TestPack0(uint8_t* tmmsg)
{
    float fbuf = 0;
    double dbuf = 0;
    tmmsg[0] = 0x01;
    //OBC累计运行时间
    memcpy(&tmmsg[1], &LOS_Time_SecFromBoot, 4);
    //配电开关状态
    memcpy(&tmmsg[5], STA_EPS.SWITCH_PD,24);
    //热控开关状态
    memcpy(&tmmsg[29], STA_EPS.SWITCH_TC, 24);
    //RTC时间
    memcpy(&tmmsg[53],&LOS_RealTime_Year,2);
    memcpy(&tmmsg[55],&LOS_RealTime_Month,1);
    memcpy(&tmmsg[56],&LOS_RealTime_Day,1);
    memcpy(&tmmsg[57],&LOS_RealTime_Hour,1);
    memcpy(&tmmsg[58],&LOS_RealTime_Minute,1);
    memcpy(&tmmsg[59],&LOS_RealTime_Second,1);
    //飞轮转速
    memcpy(&tmmsg[60],&(STA_FW[FW_X].TM_RPM),4);
    memcpy(&tmmsg[64],&(STA_FW[FW_Y].TM_RPM),4);
    memcpy(&tmmsg[68],&(STA_FW[FW_Z].TM_RPM),4);
    memcpy(&tmmsg[72],&(STA_FW[FW_S].TM_RPM),4);
    //SD卡中上注文件大小
    memcpy(&tmmsg[76],&LOS_UPFILE_SIZE,4);
    //测控至OBCCRC32值 
    memcpy(&tmmsg[80],&CRC32_TC_TRAN,4);
    //FLASH0/1标识
    tmmsg[84] = LOS_FLASH_ID;
    //母线电压、电流
    memcpy(&tmmsg[85], &STA_EPS.Vol_12V, 2);
    memcpy(&tmmsg[87], &STA_EPS.Vol_5V, 2);
    memcpy(&tmmsg[89], &STA_EPS.Vol_3V3, 2);
    memcpy(&tmmsg[91], &STA_EPS.Cur_12V, 2);
    memcpy(&tmmsg[93], &STA_EPS.Cur_5V, 2);
    memcpy(&tmmsg[95], &STA_EPS.Cur_3V3, 2);
    //工作模式
    memcpy(&tmmsg[97], &LOS_ModeAcsIn, 2);
    //OBC计算SD卡中CRC32时的偏移地址
    memcpy(&tmmsg[99], &CRC32_SD_OFFSET, 4);
    //OBC计算SD卡中的CRC32值
    memcpy(&tmmsg[103], &CRC32_SD_OBCCAL, 4);

    //当班状态
    tmmsg[109] = LOS_CPU;
    //卫星接收指令计数
    memcpy(&tmmsg[110], &LOS_TC_READ, 2);
    //卫星执行指令计数
    memcpy(&tmmsg[112], &LOS_TC_EX, 2);
    //上一条执行指令ID    
    memcpy(&tmmsg[114], &LOS_TC_LASTCMD, 4);
    //光纤陀螺角速度
    fbuf = STA_YH50.x_gyro;
    memcpy(&tmmsg[118],&fbuf,4);
    fbuf = STA_YH50.y_gyro;
    memcpy(&tmmsg[122],&fbuf,4);
    fbuf = STA_YH50.z_gyro;
    memcpy(&tmmsg[126],&fbuf,4);
    //磁强计读数
    fbuf = STA_MAGME.Field[0];
    memcpy(&tmmsg[130],&fbuf,4);
    fbuf = STA_MAGME.Field[1];
    memcpy(&tmmsg[134],&fbuf,4);
    fbuf = STA_MAGME.Field[2];
    memcpy(&tmmsg[138],&fbuf,4);
    //太敏
    fbuf = STA_SUNS.alpha;
    memcpy(&tmmsg[142],&fbuf,4);
    fbuf = STA_SUNS.beta;
    memcpy(&tmmsg[146],&fbuf,4);
    //单机通信状态:磁强计
    tmmsg[150] = STA_MAGME.MSG;
    //星敏Y四元数
    fbuf = STA_SSY.q[0];
    memcpy(&tmmsg[151],&fbuf,4);
    fbuf = STA_SSY.q[1];
    memcpy(&tmmsg[155],&fbuf,4);
    fbuf = STA_SSY.q[2];
    memcpy(&tmmsg[159],&fbuf,4);
    fbuf = STA_SSY.q[3];
    memcpy(&tmmsg[163],&fbuf,4);
    //星敏Y观测星个数
    memcpy(&tmmsg[167],&STA_SSY.StarNum, 1);
    //星敏Z观测星个数
    memcpy(&tmmsg[168],&STA_SSZ.StarNum, 1);
    //星敏Z四元数q0
    fbuf = STA_SSZ.q[0];
    memcpy(&tmmsg[169], &fbuf, 4);
    //星敏Y遥测计数
    memcpy(&tmmsg[173], &STA_SSY.TMCNT, 4);
    //星敏Z遥测计数
    memcpy(&tmmsg[177], &STA_SSZ.TMCNT, 4);
    //飞轮X遥测计数
    memcpy(&tmmsg[181], &STA_FW[FW_X].TMCNT, 4);
    //飞轮Y遥测计数
    memcpy(&tmmsg[185], &STA_FW[FW_Y].TMCNT, 4);
    //飞轮Z遥测计数
    memcpy(&tmmsg[189], &STA_FW[FW_Z].TMCNT, 4);
    //飞轮S遥测计数
    memcpy(&tmmsg[193], &STA_FW[FW_S].TMCNT, 4);
    //太敏遥测计数
    memcpy(&tmmsg[197], &STA_SUNS.TMCNT, 4);
    //光纤陀螺遥测计数
    memcpy(&tmmsg[201], &STA_YH50.TMCNT, 4);
    //磁强计遥测计数
    memcpy(&tmmsg[205], &STA_MAGME.TMCNT, 4);
    //配电扩展板遥测计数
    memcpy(&tmmsg[209], &STA_EPS.TMCNT_PD, 4);
    //热控扩展板遥测计数
    memcpy(&tmmsg[213], &STA_EPS.TMCNT_TC, 4);
    //测试模式标志位
    memcpy(&tmmsg[217], &LOS_TESTM_FLAG, 1);
    //进入姿控程序次数
    memcpy(&tmmsg[218], &AOCS_Enter_Times, 4);
    //MEMS X角速度
    memcpy(&tmmsg[222], &STA_MEMS.gyro_x, 4);
    //MEMS Y角速度
    memcpy(&tmmsg[226], &STA_MEMS.gyro_y, 4);
    //MEMS Z角速度
    memcpy(&tmmsg[230], &STA_MEMS.gyro_z, 4);
    //星敏Z四元数q1
    fbuf = STA_SSZ.q[1];
    memcpy(&tmmsg[234], &fbuf, 4);
    //星敏Z四元数q2
    fbuf = STA_SSZ.q[2];
    memcpy(&tmmsg[238], &fbuf, 4);
    //星敏Z四元数q3
    fbuf = STA_SSZ.q[3];
    memcpy(&tmmsg[242], &fbuf, 4);
    //准备读取的上注文件大小设置值
    memcpy(&tmmsg[246], &LOS_UPLOAD_FileSize, 4);
    //测控中的上注文件大小
    memcpy(&tmmsg[250],&STA_TMTC.FTP_FileSize, 4);
    //上注读取状态
    memcpy(&tmmsg[254], &LOS_UPLOAD_TC2OBC, 1);
    //星务已从测控读取到的上注程序文件大小
    memcpy(&tmmsg[255], &LOS_UPLOAD_FileReceived, 4);
    //星务待操作上注文件号
    memcpy(&tmmsg[259], &LOS_UPLOAD_FileNum, 1);
    //测控待操作文件号
    memcpy(&tmmsg[260], &STA_TMTC.FTP_FileNum, 1);
    //MAX14830状态指示
    tmmsg[261] = MAX14830_SELFSTA1[1];
    tmmsg[262] = MAX14830_SELFSTA2[1];
    //存储板状态
    tmmsg[263] = STA_SATA.cameralink_sta;
    tmmsg[264] = STA_SATA.dtspi_sta;
    tmmsg[265] = STA_SATA.cam_sta;
    tmmsg[266] = STA_SATA.sata_sw;
    tmmsg[267] = STA_SATA.baudrate;
    tmmsg[268] = STA_SATA.sata_sta;
    memcpy(&tmmsg[269], &STA_SATA.wpadd, 4);
    memcpy(&tmmsg[273], &STA_SATA.rpadd, 4);
    //存储板电流
    memcpy(&tmmsg[277], &STA_EPS.Cur_PD25940[0], 2);
    
    //MPPT电压
    memcpy(&tmmsg[280], STA_EPS.Vol_MPPT, 6);
    //充电器电压
    memcpy(&tmmsg[286], STA_EPS.Vol_Charger, 4);
    //MPPT电流
    memcpy(&tmmsg[290],STA_EPS.Cur_MPPT, 6);
    //充电器电流
    memcpy(&tmmsg[296],STA_EPS.Cur_Charger, 4);
    //MPPT温度
    memcpy(&tmmsg[300],STA_EPS.Temp_MPPT, 3);
    //充电器温度
    memcpy(&tmmsg[303],STA_EPS.Temp_Charger, 2);
    //3.3V温度
    memcpy(&tmmsg[305],&STA_EPS.Temp_3V3, 1);
    //5V温度
    memcpy(&tmmsg[306],&STA_EPS.Temp_5V, 1);
    //12V温度
    memcpy(&tmmsg[307],&STA_EPS.Temp_12V, 1);
    //分阵输入电压
    memcpy(&tmmsg[308],STA_EPS.Vol_SA, 6);
    //飞轮XYZS电流（25940）
    memcpy(&tmmsg[314], &STA_EPS.Cur_PD25940[12], 8);
    //星敏YZ电流（25940）
    memcpy(&tmmsg[322], &STA_EPS.Cur_TC25940[12], 4);
    //太敏电流（25940）
    memcpy(&tmmsg[326], &STA_EPS.Cur_PD25940[11], 2);
    //光纤陀螺电流（25940）
    memcpy(&tmmsg[328], &STA_EPS.Cur_TC25940[15], 2);
    //磁强计电流（25940）
    memcpy(&tmmsg[330], &STA_EPS.Cur_TC25940[14], 2);
    //MEMS电流（25940）
    memcpy(&tmmsg[332], &STA_EPS.Cur_PD25940[19], 2);
    //GNSS电流（25940）
    memcpy(&tmmsg[334], &STA_EPS.Cur_PD25940[7], 2);
    //配电执行计数
    memcpy(&tmmsg[336], &STA_EPS.PD_CMDCNT, 2);
    //热控执行计数
    memcpy(&tmmsg[338], &STA_EPS.TC_CMDCNT, 2);
    //发送配电遥测请求计数
    memcpy(&tmmsg[340], &PD_TCCNT, 2);
    //发送热控遥测请求计数
    memcpy(&tmmsg[342], &TC_TCCNT, 2);
    //启动计数
    memcpy(&tmmsg[344], &DWDRST_CNT, 4);
    //配电错误指令计数
    memcpy(&tmmsg[348], &STA_EPS.PD_CMDERRCNT, 2);
    //热控错误指令计数
    memcpy(&tmmsg[350], &STA_EPS.TC_CMDERRCNT, 2);
    //配电遥测错误计数
    tmmsg[352] = STA_EPS.PD_CRCERRCNT;
    //热控遥测错误计数
    tmmsg[353] = STA_EPS.TC_CRCERRCNT;
    //存储板遥测计数
    memcpy(&tmmsg[354], &STA_SATA.tmcnt, 4);
    //小相机
    tmmsg[358] = STA_CMOS.bright;
    tmmsg[359] = STA_CMOS.contrast;
    tmmsg[360] = STA_CMOS.sharp;
    tmmsg[361] = STA_CMOS.ev;
    tmmsg[362] = STA_CMOS.compress;
    tmmsg[363] = STA_CMOS.color;
    //星敏Y角速度
    fbuf = STA_SSY.angvel[0];
    memcpy(&tmmsg[364], &fbuf, 4);
    fbuf = STA_SSY.angvel[1];
    memcpy(&tmmsg[368], &fbuf, 4);
    fbuf = STA_SSY.angvel[2];
    memcpy(&tmmsg[372], &fbuf, 4);
    //星敏Z角速度
    fbuf = STA_SSZ.angvel[0];
    memcpy(&tmmsg[376], &fbuf, 4);
    fbuf = STA_SSZ.angvel[1];
    memcpy(&tmmsg[380], &fbuf, 4);
    fbuf = STA_SSZ.angvel[2];
    memcpy(&tmmsg[384], &fbuf, 4);
    //高分相机
    memcpy(&tmmsg[388], STA_CIOMPCam.TMMSG, 29);
    //高分相机电流
    memcpy(&tmmsg[417], &STA_EPS.Cur_PD25940[16], 2);
    //SD卡存储CRC32值
    memcpy(&tmmsg[419], &CRC32_SD_SAVE, 4);
    //从SD卡向FLASH写入时剩余文件大小
    memcpy(&tmmsg[423], &LOS_SD2FLASH_SIZE, 4);
    //配电扩展板电流
    memcpy(&tmmsg[427], STA_EPS.Cur_PD25940, 48);
    //热控扩展板电流
    memcpy(&tmmsg[475], STA_EPS.Cur_TC25940, 48);
    //加热带1-13温度上限
    memcpy(&tmmsg[523], STA_EPS.Heater_UP, 13);
    //加热带1-13温度下限
    memcpy(&tmmsg[536], STA_EPS.Heater_LOW, 13);
    //热敏电阻1-32
    memcpy(&tmmsg[549], &STA_EPS.Temp_Ther[0], 32);
    //导航增强
    memcpy(&tmmsg[581], navi_dabuf, 24);
    // //热敏电阻1
    // memcpy(&tmmsg[549], &STA_EPS.Temp_Ther[0], 1);
    // //热敏电阻3
    // memcpy(&tmmsg[550], &STA_EPS.Temp_Ther[2], 1);
    // //热敏电阻6
    // memcpy(&tmmsg[551], &STA_EPS.Temp_Ther[5], 1);
    // //热敏电阻7
    // memcpy(&tmmsg[552], &STA_EPS.Temp_Ther[6], 1);
    // //热敏电阻10
    // memcpy(&tmmsg[553], &STA_EPS.Temp_Ther[9], 1);
    // //热敏电阻11
    // memcpy(&tmmsg[554], &STA_EPS.Temp_Ther[10], 1);
    // //热敏电阻14
    // memcpy(&tmmsg[555], &STA_EPS.Temp_Ther[13], 1);
    // //热敏电阻20-32
    // memcpy(&tmmsg[556], &STA_EPS.Temp_Ther[19], 13);
}
